﻿#include <learn/math/matrix_solver.h>

using namespace xi;
using namespace xi::math;

namespace
{

std::mt19937 g_gen(std::random_device{}());
std::uniform_real_distribution<double> g_dis(0, 10);

MatrixXd solve_lower_diag1(const MatrixXd &A, MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    std::vector<double> sum(m);
    for (std::size_t i = 1; i < n; i++)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = 0; j < i; j++)
                sum[k] += A(i, j) * b(k, j);

            b(k, i) -= sum[k];
        }
    }
    return b;
}

MatrixXd solve_upper_diag1(const MatrixXd &A, MatrixXd &&b)
{
    // 上三角求解器允许求解非方阵，只考虑矩阵 A 的上三角部分
    ASSERT_INFO(A.rows() >= b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.cols();

    // 解线性方程组 Ax = b
    std::vector<double> sum(m);
    for (int i = n - 1; i >= 0; i--)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = i + 1; j < n; j++)
                sum[k] += A(i, j) * b(k, j);

            b(k, i) -= sum[k];
        }
    }
    return b;
}

} // namespace

Matrix_Solver::Matrix_Solver(const MatrixXd &A) : A(A), m_status(Status::Uninitialized)
{
}

Matrix_Solver::Matrix_Solver(MatrixXd &&A) : A(std::move(A)), m_status(Status::Uninitialized)
{
}

Matrix_Solver::Status Matrix_Solver::status() const
{
    return m_status;
}

Matrix_Solver::Status &Matrix_Solver::status()
{
    return m_status;
}

MatrixXd &&Matrix_Solver::move_matrix()
{
    return std::move(A);
}

void Matrix_Solver::perform()
{
    // do nothing
    status() = Status::Success;
}

Matrix_L_Solver::Matrix_L_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_L_Solver::Matrix_L_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

MatrixXd Matrix_L_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    for (std::size_t k = 0; k < m; k++)
        b(k, 0) /= A(0, 0);

    std::vector<double> sum(m);
    for (std::size_t i = 1; i < n; i++)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = 0; j < i; j++)
                sum[k] += A(i, j) * b(k, j);

            ASSERT_INFO(std::abs(A(i, i)) > std::numeric_limits<double>::epsilon(), "MatrixXd is singular");
            b(k, i) = (b(k, i) - sum[k]) / A(i, i);
        }
    }
    return b;
}

std::array<MatrixXd, 3> Matrix_L_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd L(col, col);
    L.set_random();

    for (std::size_t i = 0; i < col; i++)
    {
        for (std::size_t j = 0; j < col; j++)
        {
            if (i < j)
                L(i, j) = 0;
        }
    }

    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (L * x.transposed()).transposed();
    return {L, x, b};
}

Matrix_U_Solver::Matrix_U_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_U_Solver::Matrix_U_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

MatrixXd Matrix_U_Solver::solve(MatrixXd &&b)
{
    // 上三角求解器允许求解非方阵，只考虑矩阵 A 的上三角部分
    ASSERT_INFO(A.rows() >= b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.cols();

    // 解线性方程组 Ax = b
    std::vector<double> sum(m);
    for (int i = n - 1; i >= 0; i--)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = i + 1; j < n; j++)
                sum[k] += A(i, j) * b(k, j);

            ASSERT_INFO(std::abs(A(i, i)) > std::numeric_limits<double>::epsilon(), "MatrixXd is singular");
            b(k, i) = (b(k, i) - sum[k]) / A(i, i);
        }
    }
    return b;
}

std::array<MatrixXd, 3> Matrix_U_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd U(col, col);
    U.set_random();

    for (std::size_t i = 0; i < col; i++)
    {
        for (std::size_t j = 0; j < col; j++)
        {
            if (i > j)
                U(i, j) = 0;
        }
    }

    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (U * x.transposed()).transposed();
    return {U, x, b};
}

Matrix_LU_Solver::Matrix_LU_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_LU_Solver::Matrix_LU_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_LU_Solver::perform()
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");

    std::size_t n = A.rows();
    for (std::size_t i = 0; i < n; i++)
    {
        // 确保对角元足够大，否则算法失败
        if (std::abs(A(i, i)) < std::numeric_limits<double>::epsilon())
        {
            status() = Status::Failure;
            return;
        }

        // 正常的高斯消去法
        for (std::size_t j = i + 1; j < n; j++)
        {
            A(j, i) /= A(i, i);
            for (std::size_t k = i + 1; k < n; k++)
                A(j, k) -= A(i, k) * A(j, i);
        }
    }
    status() = Status::Success;
}

MatrixXd Matrix_LU_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "LU decomposition not computed");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    auto x = solve_lower_diag1(A, std::move(b));
    Matrix_U_Solver solver(std::move(A));
    auto z = solver.solve(std::move(x));
    A = solver.move_matrix();
    return z;
}

std::array<MatrixXd, 3> Matrix_LU_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd L(col, col);
    MatrixXd U(col, col);
    L.set_random();
    U.set_random();
    for (std::size_t i = 0; i < col; i++)
    {
        for (std::size_t j = 0; j < col; j++)
        {
            if (i > j)
                U(i, j) = 0;
            else if (i == j)
                L(i, j) = 1;
            else
                L(i, j) = 0;
        }
    }

    MatrixXd A = L * U;
    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (A * x.transposed()).transposed();
    return {A, x, b};
}

Matrix_PTLU_Solver::Matrix_PTLU_Solver(const MatrixXd &A) : Matrix_Solver(A), m_perm(A.rows())
{
}

Matrix_PTLU_Solver::Matrix_PTLU_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A)), m_perm(A.rows())
{
}

void Matrix_PTLU_Solver::perform()
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");

    // 填充置换向量
    std::iota(m_perm.begin(), m_perm.end(), 0);

    std::size_t n = A.rows();
    for (std::size_t i = 0; i < n; i++)
    {
        // 列主元处理
        double tmp, max = 0;   // 记录最大值
        std::size_t index = i; // 记录要交换的主元位置

        // 选取列主元
        for (std::size_t p = i; p < n; p++)
        {
            tmp = std::abs(A(p, i));
            if (tmp > max)
            {
                max = tmp;
                index = p;
            }
        }

        // 交换主元
        for (std::size_t q = 0; q < n; q++)
            std::swap(A(index, q), A(i, q));

        // 确保对角元足够大，否则不可逆
        if (std::abs(A(i, i)) < std::numeric_limits<double>::epsilon())
        {
            status() = Status::Singular;
            return;
        }

        // 交换指标，记录置换
        std::swap(m_perm[i], m_perm[index]);

        // 正常的高斯消去法
        for (std::size_t j = i + 1; j < n; j++)
        {
            A(j, i) /= A(i, i);
            for (std::size_t k = i + 1; k < n; k++)
                A(j, k) -= A(i, k) * A(j, i);
        }
    }
    status() = Status::Success;
}

MatrixXd Matrix_PTLU_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "PTLU decomposition not computed");

    std::size_t m = b.rows();
    std::size_t n = A.rows();
    MatrixXd b1(m, n);

    // 对 b 进行置换
    for (std::size_t k = 0; k < m; k++)
    {
        for (std::size_t i = 0; i < n; i++)
            b1(k, i) = b(k, m_perm[i]);
    }

    auto x = solve_lower_diag1(A, std::move(b1));
    Matrix_U_Solver solver(std::move(A));
    auto z = solver.solve(std::move(x));
    A = solver.move_matrix();
    return z;
}

std::array<MatrixXd, 3> Matrix_PTLU_Solver::example(std::size_t row, std::size_t col)
{
    return Matrix_LU_Solver::example(row, col);
}

Matrix_LLT_Solver::Matrix_LLT_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_LLT_Solver::Matrix_LLT_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_LLT_Solver::perform()
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");

    std::size_t n = A.rows();
    for (std::size_t i = 0; i < n; i++)
    {
        double sum = 0;
        for (std::size_t p = 0; p < i; p++)
            sum += A(i, p) * A(i, p);
        A(i, i) = std::sqrt(A(i, i) - sum);

        // 确保对角元足够大，否则不可分解
        if (std::abs(A(i, i)) < std::numeric_limits<double>::epsilon())
        {
            status() = Status::Failure;
            return;
        }

        for (std::size_t j = i + 1; j < n; j++)
        {
            sum = 0;
            for (std::size_t k = 0; k < i; k++)
                sum += A(j, k) * A(i, k);

            A(j, i) = (A(j, i) - sum) / A(i, i);
        }
    }
    status() = Status::Success;
}

MatrixXd Matrix_LLT_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "LLT decomposition not computed");

    Matrix_L_Solver lsolver(std::move(A));
    auto x = lsolver.solve(std::move(b));
    A = lsolver.move_matrix();

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    // 解线性方程组 Ax = b
    std::vector<double> sum(m);
    for (int i = n - 1; i >= 0; i--)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = i + 1; j < n; j++)
                sum[k] += A(j, i) * x(k, j);

            x(k, i) = (x(k, i) - sum[k]) / A(i, i);
        }
    }
    return x;
}

std::array<MatrixXd, 3> Matrix_LLT_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd L(col, col);
    L.set_random();
    for (std::size_t i = 0; i < col; i++)
    {
        for (std::size_t j = i; j < col; j++)
        {
            if (i == j)
                L(i, j) = g_dis(g_gen);
            else
                L(i, j) = 0;
        }
    }

    MatrixXd A = L * L.transposed();
    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (A * x.transposed()).transposed();
    return {A, x, b};
}

Matrix_LDLT_Solver::Matrix_LDLT_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_LDLT_Solver::Matrix_LDLT_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_LDLT_Solver::perform()
{
    std::size_t n = A.rows();
    for (std::size_t i = 0; i < n; i++)
    {
        double sum = 0;
        for (std::size_t p = 0; p < i; p++)
            sum += A(i, p) * A(i, p) * A(p, p);

        A(i, i) -= sum;

        // 确保对角元足够大，否则不可分解
        if (std::abs(A(i, i)) < std::numeric_limits<double>::epsilon())
        {
            status() = Status::Failure;
            return;
        }

        for (std::size_t j = i + 1; j < n; j++)
        {
            sum = 0;
            for (std::size_t k = 0; k < i; k++)
                sum += A(j, k) * A(i, k) * A(k, k);

            A(j, i) -= sum;
            A(j, i) /= A(i, i);
        }
    }

    status() = Status::Success;
}

MatrixXd Matrix_LDLT_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "LDLT decomposition not computed");

    auto x = solve_lower_diag1(A, std::move(b));
    std::size_t m = b.rows();
    std::size_t n = A.rows();

    // 解线性方程组 Ax = b
    std::vector<double> sum(m);
    for (int i = n - 1; i >= 0; i--)
    {
        std::fill(sum.begin(), sum.end(), 0);
        for (std::size_t k = 0; k < m; k++)
        {
            for (std::size_t j = i + 1; j < n; j++)
                sum[k] += A(j, i) * x(k, j);

            x(k, i) = x(k, i) / A(i, i) - sum[k];
        }
    }

    return x;
}

std::array<MatrixXd, 3> Matrix_LDLT_Solver::example(std::size_t row, std::size_t col)
{
    return Matrix_LLT_Solver::example(row, col);
}

Matrix_Thomas_Solver::Matrix_Thomas_Solver(const Tridiagonal &A) : Matrix_Solver(A)
{
}

Matrix_Thomas_Solver::Matrix_Thomas_Solver(Tridiagonal &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_Thomas_Solver::perform()
{
    // 强制转换为三对角阵
    Tridiagonal &A = static_cast<Tridiagonal &>(this->A);

    // 三对角阵分解
    std::size_t n = A.cols();
    for (std::size_t i = 1; i < n; i++)
    {
        // 确保对角元足够大，否则不可分解
        if (std::abs(A(i - 1, i - 1)) < std::numeric_limits<double>::epsilon())
        {
            status() = Status::Failure;
            return;
        }

        A(i - 1, i) /= A(i - 1, i - 1);
        A(i, i) -= A(i, i - 1) * A(i - 1, i);
    }

    if (std::abs(A(n - 1, n - 1)) < std::numeric_limits<double>::epsilon())
        status() = Status::Singular;
    else
        status() = Status::Success;
}

MatrixXd Matrix_Thomas_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.cols() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "LU decomposition not computed");

    // 强制转换为三对角阵
    Tridiagonal &A = static_cast<Tridiagonal &>(this->A);

    std::size_t m = b.rows();
    std::size_t n = A.cols();

    // 首先解下三角阵
    for (std::size_t k = 0; k < m; k++)
        b(k, 0) /= A(0, 0);

    for (std::size_t i = 1; i < n; i++)
    {
        for (std::size_t k = 0; k < m; k++)
            b(k, i) = (b(k, i) - A(i, i - 1) * b(k, i - 1)) / A(i, i);
    }

    // 解上三角阵
    for (int i = n - 2; i >= 0; i--)
    {
        for (std::size_t k = 0; k < m; k++)
            b(k, i) -= A(i, i + 1) * b(k, i + 1);
    }
    return b;
}

std::tuple<Tridiagonal, MatrixXd, MatrixXd> Matrix_Thomas_Solver::example(std::size_t row, std::size_t col)
{
    Tridiagonal L(col);
    Tridiagonal U(col);
    L.set_random();
    U.set_random();
    U(0, 0) = 1;
    for (std::size_t i = 1; i < col; i++)
    {
        U(i, i) = 1;
        U(i, i - 1) = L(i - 1, i) = 0;
    }

    MatrixXd A = (L * U).to_matrix();
    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (A * x.transposed()).transposed();
    return {Tridiagonal::from_matrix(A), x, b};
}

Matrix_Blind_Solver::Matrix_Blind_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_Blind_Solver::Matrix_Blind_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_Blind_Solver::perform()
{
    // 只需要使用普通的 LU 分解即可，不过可能会导致精度下降
    Matrix_LU_Solver solver(std::move(A));
    solver.perform();
    A = solver.move_matrix();
    status() = solver.status();
}

MatrixXd Matrix_Blind_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(b.rows() == 1, "MatrixXd must be a row vector");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "LU decomposition not computed");

    std::size_t n = A.rows();
    MatrixXd AT = A.transposed();
    VectorXd x = b;
    while (true)
    {
        // w = B * x = f(x)
        Matrix_L_Solver lsolver(std::move(AT));
        VectorXd w = lsolver.solve(std::move(b));
        AT = lsolver.move_matrix();
        w = solve_upper_diag1(AT, std::move(w));

        // 符号向量
        MatrixXd v(1, n);
        for (std::size_t i = 0; i < n; i++)
            v(0, i) = w[i] > 0 ? 1 : -1;

        // z = BT * v = J(x)
        VectorXd z = solve_lower_diag1(A, std::move(v));
        Matrix_U_Solver usolver(std::move(A));
        z = usolver.solve(std::move(z));
        A = usolver.move_matrix();

        // 比较无穷范数 z 与 zT * x
        // 由于 z 是行向量，所以使用 1 范数
        if (z.norm(1) <= dot(z, x))
            return w;

        // 将 x 设为 z 最大模位置为 1 的单位向量
        x.set_zero();
        std::size_t maxId = 0;
        double max = 0;
        for (std::size_t i = 0; i < n; i++)
        {
            if (std::abs(z[i]) > max)
            {
                max = std::abs(z[i]);
                maxId = i;
            }
        }
        x[maxId] = 1;
        b = x;
    }
    return b;
}

std::array<MatrixXd, 3> Matrix_Blind_Solver::example(std::size_t n)
{
    MatrixXd L(n, n);
    MatrixXd U(n, n);
    L.set_random();
    U.set_random();
    for (std::size_t i = 0; i < n; i++)
    {
        for (std::size_t j = 0; j < n; j++)
        {
            if (i > j)
                U(i, j) = 0;
            else if (i == j)
                L(i, j) = 1;
            else
                L(i, j) = 0;
        }
    }

    MatrixXd A = L * U;
    MatrixXd x(1, n);
    x.set_zero();
    x(0, 0) = 1;
    return {A, A.inverted(), x};
}

Matrix_Optimal_Solver::Matrix_Optimal_Solver(const MatrixXd &A, const MatrixXd &x) : Matrix_Solver(A), x(x)
{
}

Matrix_Optimal_Solver::Matrix_Optimal_Solver(MatrixXd &&A, MatrixXd &&x) : Matrix_Solver(std::move(A)), x(std::move(x))
{
}

MatrixXd Matrix_Optimal_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    // 估计 A^-1 的无穷范数
    Matrix_Blind_Solver solver(A);
    solver.perform();
    MatrixXd w(1, A.rows());
    w.set_zero();
    w(0, 0) = 1;
    w = solver.solve(std::move(w));
    double nu = w.norm(0);
    double mu = A.norm(0);
    double beta = b.norm(1);
    A.transpose(); // 注意需要转置

    // 直接将 Blind 中的分解结果传入
    Matrix_LU_Solver lusolver(solver.move_matrix());
    lusolver.status() = solver.status();

    int count = Numeric::iter;
    double rho = 1;
    do
    {
        MatrixXd r = b - x * A;
        double gamma = r.norm(1);
        double rho = nu * mu * gamma / beta;
        r = lusolver.solve(std::move(r));
        x += r;
    } while (--count > 0 && rho > Numeric::half);

    return x;
}

std::array<MatrixXd, 3> Matrix_Optimal_Solver::example(std::size_t row, std::size_t col)
{
    return Matrix_LU_Solver::example(row, col);
}

Matrix_Jacobi_Solver::Matrix_Jacobi_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_Jacobi_Solver::Matrix_Jacobi_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

MatrixXd Matrix_Jacobi_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    // x = D1(L+U) * x + D1 * b
    MatrixXd x(m, n), y(m, n);
    x.set_zero();

    double dx = 0;
    do
    {
        // 分步迭代过程
        for (std::size_t i = 0; i < n; i++)
        {
            for (std::size_t k = 0; k < m; k++)
            {
                y(k, i) = 0;
                for (std::size_t j = 0; j < i; j++)
                    y(k, i) -= A(i, j) * x(k, j);

                for (std::size_t j = i + 1; j < n; j++)
                    y(k, i) -= A(i, j) * x(k, j);

                y(k, i) = (y(k, i) + b(k, i)) / A(i, i);
            }
        }
        dx = (y - x).norm();
        x = y;
    } while (dx > epsilon);

    return x;
}

std::array<MatrixXd, 3> Matrix_Jacobi_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd A = MatrixXd::strictly_diagonally_dominant(col);
    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (A * x.transposed()).transposed();
    return {A, x, b};
}

Matrix_GaussSeidel_Solver::Matrix_GaussSeidel_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_GaussSeidel_Solver::Matrix_GaussSeidel_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

MatrixXd Matrix_GaussSeidel_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    // x = (D-L)1U * x + (D-L)1 * b
    MatrixXd x(m, n);
    x.set_zero();

    double dx = 0;
    do
    {
        // 分步迭代过程
        dx = 0;
        for (std::size_t i = 0; i < n; i++)
        {
            for (std::size_t k = 0; k < m; k++)
            {
                double sum = 0;
                for (std::size_t j = 0; j < i; j++)
                    sum -= A(i, j) * x(k, j);

                for (std::size_t j = i + 1; j < n; j++)
                    sum -= A(i, j) * x(k, j);

                sum = (sum + b(k, i)) / A(i, i);
                dx = std::max(dx, std::abs(x(k, i) - sum));
                x(k, i) = sum;
            }
        }
    } while (dx > epsilon);

    return x;
}

std::array<MatrixXd, 3> Matrix_GaussSeidel_Solver::example(std::size_t row, std::size_t col)
{
    return Matrix_Jacobi_Solver::example(row, col);
}

Matrix_SOR_Solver::Matrix_SOR_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_SOR_Solver::Matrix_SOR_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

MatrixXd Matrix_SOR_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");

    std::size_t m = b.rows();
    std::size_t n = A.rows();

    // x = Lw * x + w(D-wL)1 * b
    MatrixXd x(m, n);
    x.set_zero();

    double dx = 0;
    do
    {
        // 分步迭代过程
        dx = 0;
        for (std::size_t i = 0; i < n; i++)
        {
            for (std::size_t k = 0; k < m; k++)
            {
                double sum = 0;
                for (std::size_t j = 0; j < i; j++)
                    sum -= A(i, j) * x(k, j);

                for (std::size_t j = i + 1; j < n; j++)
                    sum -= A(i, j) * x(k, j);

                sum = (1 - omega) * x(k, i) + omega * (sum + b(k, i)) / A(i, i);
                dx = std::max(dx, std::abs(x(k, i) - sum));
                x(k, i) = sum;
            }
        }
    } while (dx > epsilon);

    return x;
}

std::array<MatrixXd, 3> Matrix_SOR_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd A = MatrixXd::symmetric_positive_definite(col);
    MatrixXd x(row, col);
    x.set_random();
    MatrixXd b = (A * x.transposed()).transposed();
    return {A, x, b};
}

Matrix_QR_Solver::Matrix_QR_Solver(const MatrixXd &A) : Matrix_Solver(A), beta(A.cols())
{
}

Matrix_QR_Solver::Matrix_QR_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A)), beta(A.cols())
{
}

void Matrix_QR_Solver::perform()
{
    std::size_t row = A.rows();
    std::size_t col = A.cols();

    for (int i = 0; i < col; i++)
    {
        VectorXd v(row - i);
        for (std::size_t j = i; j < row; j++)
            v[j - i] = A(j, i);

        beta[i] = householder(v.data(), v.size());
        VectorXd w(col - i);

        // get w = beta * AT * v
        for (std::size_t k = i; k < col; k++)
        {
            double sum = 0;
            for (std::size_t p = i; p < row; p++)
                sum += A(p, k) * v[p - i];

            // note: it is w[k-i], not w[k]
            w[k - i] = beta[i] * sum;
        }

        // get HA = A - v * wT
        for (std::size_t k = i; k < row; k++)
        {
            for (std::size_t p = i; p < col; p++)
            {
                if (p == i && k > i)
                    A(k, p) = v[k - i];
                else
                    A(k, p) -= v[k - i] * w[p - i];
            }
        }
    }

    status() = Status::Success;
}

MatrixXd Matrix_QR_Solver::solve(MatrixXd &&b)
{
    ASSERT_INFO(A.rows() == b.cols(), "MatrixXd and vector not aligned");
    ASSERT_INFO(status() == Status::Success, "QR decomposition not computed");

    std::size_t row = A.rows();
    std::size_t col = A.cols();
    std::size_t n = b.rows();

    // householder 变换作用于 b
    // Hb = b - beta * v * v^T * b = b - (beta * v^T * b) * v
    for (std::size_t r = 0; r < n; r++)
    {
        for (std::size_t i = 0; i < col; i++)
        {
            double sum = b(r, i);
            for (std::size_t j = i + 1; j < row; j++)
                sum += A(j, i) * b(r, j);

            sum *= beta[i];
            b(r, i) -= sum;
            for (std::size_t j = i + 1; j < row; j++)
                b(r, j) -= A(j, i) * sum;
        }
    }

    Matrix_U_Solver usolver(std::move(A));
    auto x = usolver.solve(std::move(b));
    x.resize(n, col, true);
    A = usolver.move_matrix();

    return x;
}

std::array<MatrixXd, 3> Matrix_QR_Solver::example(std::size_t row, std::size_t col)
{
    MatrixXd L(col, col);
    MatrixXd U(col, col);
    L.set_random();
    U.set_random();
    for (std::size_t i = 0; i < col; i++)
    {
        for (std::size_t j = 0; j < col; j++)
        {
            if (i > j)
                U(i, j) = 0;
            else if (i == j)
                L(i, j) = 1;
            else
                L(i, j) = 0;
        }
    }

    MatrixXd A = L * U;
    A.resize(col, col / 2);
    MatrixXd b(row, col);
    b.set_random();
    Matrix_LDLT_Solver solver(A.transposed() * A);
    solver.perform();
    auto z = solver.solve(b * A);
    return {A, z, b};
}

Matrix_SymEigen_Solver::Matrix_SymEigen_Solver(const MatrixXd &A) : Matrix_Solver(A)
{
}

Matrix_SymEigen_Solver::Matrix_SymEigen_Solver(MatrixXd &&A) : Matrix_Solver(std::move(A))
{
}

void Matrix_SymEigen_Solver::perform()
{
    status() = Status::Success;
}

MatrixXd Matrix_SymEigen_Solver::solve(MatrixXd &&sig)
{
    ASSERT_INFO(A.rows() == A.cols(), "MatrixXd must be square");
    ASSERT_INFO(status() == Status::Success, "Jacobi decomposition not computed");

    // 过关 Jacobi 方法。将 A 变换为特征值对角阵，返回正交阵 Q，每一列是特征向量，sigma > n 为关值减小的比例
    std::size_t n = A.cols();

    // Q 初始化为单位阵，用于累计正交变换
    auto Q = MatrixXd::identity(n);

    // delta_k = delta_{k-1} / sigma
    double sigma = sig(0, 0);
    if (sigma < n)
        return Q;

    std::size_t count = 0;
    double delta = std::sqrt(A.norm(2));
    while (delta > Numeric::epsilon && count++ < Numeric::iter)
    {
        bool check = true;
        for (std::size_t i = 0; i < n; i++)
        {
            for (std::size_t j = i + 1; j < n; j++)
            {
                if (std::abs(A(i, j)) > delta)
                {
                    Q = rotate(std::move(Q), i, j);
                    check = false;
                }
            }
        }

        // 如果所有分量都小于关值，就减小关值
        if (check)
            delta /= sigma;
    }
    return Q;
}

MatrixXd Matrix_SymEigen_Solver::rotate(MatrixXd &&Q, std::size_t p, std::size_t q)
{
    std::size_t n = A.cols();

    double cs[2];
    if (A(p, q) == 0)
    {
        cs[0] = 1;
        cs[1] = 0;
    }
    else
    {
        // t*t + 2*tau*t - 1 = 0
        double tau = (A(q, q) - A(p, p)) / (2 * A(p, q));
        double t = sign(tau) / (std::abs(tau) + std::sqrt(1 + tau * tau));
        cs[0] = 1 / (std::sqrt(1 + t * t));
        cs[1] = t * cs[0];
    }

    // JT * A
    for (std::size_t j = 0; j < n; j++)
    {
        double tmp = A(p, j);
        A(p, j) = tmp * cs[0] - A(q, j) * cs[1];
        A(q, j) = cs[1] * tmp + A(q, j) * cs[0];
    }

    // JT * A * J, note: here J is not a symmetical matrix
    // Q * J
    for (std::size_t j = 0; j < n; j++)
    {
        double tmp = A(j, p);
        A(j, p) = tmp * cs[0] - A(j, q) * cs[1];
        A(j, q) = cs[1] * tmp + A(j, q) * cs[0];

        tmp = Q(j, p);
        Q(j, p) = tmp * cs[0] - Q(j, q) * cs[1];
        Q(j, q) = cs[1] * tmp + Q(j, q) * cs[0];
    }

    return Q;
}